/**

Title   : MM3AWidget.cpp
Author  : Manikantan Nambi
URL :

Description : Function definitions for MM3AWidget.h
Created : 1/1/11
Modified   : 2/14/11

*/

#include "MM3AWidget.h"

extern Matrix XinPix;
extern int Scount;
extern CvMat *position_points;
extern CvPoint probeTip;

const double q3Range = 12.0;
const double q2Range = 2 * M_PI / 3;


/**  \brief   MM3AWidget constructor
*
*   Parameters:     parent widget
*   Returns:    None
*/
MM3AWidget::MM3AWidget(QWidget *parent)  {
    
    qDebug()<<"MM3AWidget:MM3AWidget(): Creating Object";
    estCorner = Matrix(4,2);

    // zero matrices
    zero3x1 = Matrix(3,1);
    zero3x3 = Matrix(3,3);
    zero3x1 << 0 << 0 << 0;
    zero3x3 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0 << 0;

    // default values for step size in micro radians and mm.
    stepSizeA = 0.000058;
    stepSizeB = 0.000090;
    stepSizeC = 0.000806;
    // Model parameters
    // initializing and assigning step size values

    g1q30t0p0Plus = stepSizeA;           // gamma 1 q3 0 theta 0 phi 0 plus direction
    g1q30t0p0Minus = stepSizeA;

    g2q30t0p0Plus = stepSizeB;
    g2q312t0p0Plus  = stepSizeB;
    g2q30t_90p0Plus  = 115.0; // stepSizeB;
    g2q312t_90p0Plus  = stepSizeB;
    g2q30t0p0Minus  = stepSizeB;
    g2q312t0p0Minus  = stepSizeB;
    g2q30t_90p0Minus  = 95.0; // stepSizeB;
    g2q312t_90p0Minus  = stepSizeB;

    g3q20t0p0Plus  = stepSizeC;
    g3q2_90t0p0Plus  = stepSizeC;
    g3q20t_90p0Plus  = stepSizeC;
    g3q20t0p0Minus  = stepSizeC;
    g3q2_90t0p0Minus  = stepSizeC;
    g3q20t_90p0Minus  = stepSizeC;

    // Values taken from Damani thesis Fig. 4.13 for pilot testing.
    aPlus = 972; //  g3q2_90t0p0Plus;
    bPlus = 0.27; // 1 - g3q20t90p0Plus/aPlus;
    cPlus = 375; // aPlus - (g3q20t0p0Plus / (1 - bPlus));

    aMinus = 899; // g3q2_90t0p0Minus;
    bMinus = 0.25; // 1 - g3q20t90p0Minus/aMinus;
    cMinus = 436; // aMinus - (g3q20t0p0Minus / (1 - bMinus));

    //    d2q30Plus = 3.41 * pow((g2q30t0p0Plus - g2q30t_90p0Plus), 2) + 0.025 * (g2q30t0p0Plus - g2q30t_90p0Plus) + 6.27 * pow(1, -7);
    //    d2q30Minus = 3.41 * pow((g2q30t0p0Minus - g2q30t_90p0Minus), 2) + 0.025 * (g2q30t0p0Minus - g2q30t_90p0Minus) + 6.27 * pow(1, -7);

    d2q30Plus = -8.15;
    d2q30Minus = 3.85;


    //    d2q312Plus = 3.41 * pow((g2q312t0p0Plus - g2q312t_90p0Plus), 2) + 0.025 * (g2q312t0p0Plus - g2q312t_90p0Plus) + 6.27 * pow(1, -7);
    //    d2q312Minus = 3.41 * pow((g2q312t0p0Minus - g2q312t_90p0Minus), 2) + 0.025 * (g2q312t0p0Minus - g2q312t_90p0Minus) + 6.27 * pow(1, -7);

    d2q312Plus = -19.40;
    d2q312Minus = 23.94;

    P = zero3x3;

    P(1,1) = stepSizeA;     // link 3 is along x-axis
    P(2,2) = stepSizeB;     // link 1 is along x and y-axis
    P(3,3) = stepSizeC;     // because link 2 controls the x and z co-ordinate

    recPoints = Matrix(2,4);            // initializing record points matrix
    rCount = 1;

    simulate_status = 0;                // set simulate status

    port = new SerialWidget();     // initializing serial port

    // defining model parameters
    mAaclk = 15300;
    mAclk =15000;
    c =0.0;
    L2 = 0.031;
    mA= 0.354;

    mCin = 1200 ;
    mCout= 1223;
    cAaclk = 0;
    cAclk = 0;
    cCin = 0;
    cCout = 0;

    mC = 0 ;
    mass = 0.2;

    // initializing step counts to 0
    stepCountA = 0;
    stepCountB = 0;
    stepCountC = 0;
    
    // scalingFactor = 69.3277;        // defining scaling factor

    Scount = 2;                             // counting number of points

    // defining DH parameters for the manipulator
    tool = 15 ;
    toolOffset = 10.0;
    a1 = 17.35;
    a2 = 0;
    a3 = 0;
    d1 = 0;
    d2 = 0;
    d3 = 21.51 + toolOffset; // add displacement of third link to this variable and add end effector length
    alpha1 = 0;
    alpha2 = 1.57;
    alpha3 = 0;




    dQ = zero3x1(3,1);
    q = zero3x1;                        // initial joint configuration of mm3a
    qf = zero3x1;                       // final joint configuration of mm3a
    xi = zero3x1;                       // initial position of mm3a
    xf = zero3x1;                       // final position of mm3a
    dx = zero3x1;                      // change in position
    dq = zero3x1;                      // change in joint configuration
    center = zero3x1;                // center co-ordinates in pixels
    xPix = zero3x1;                   // current position in pixels

    // initializing q
    q(1,1) = 0.0;
    q(2,1) = -1.57;
    q(3,1) = 0.0;

    J = zero3x3;
    // initializing Jacobian values
    J(1,1) = - (a1 - (d3+q(3,1))*sin(q(2,1)))*(-sin(theta1)); // + toolOffset*cos(theta1);
    J(1,2) = - (d3 + q(3,1)) * cos(q(1,1)) * cos(q(2,1));
    J(1,3) = - cos(q(1,1)) * sin(q(2,1));
    J(2,1) = (a1 - (d3+q(3,1))*sin(q(2,1))) * cos(q(1,1)) ; //+ toolOffset*sin(theta1);
    J(2,2) = - (d3 + q(3,1)) * sin(q(1,1)) * cos(q(2,1));
    J(2,3) = - sin(q(1,1)) * sin(q(2,1));
    J(3,1) = 0.0;
    J(3,2) = - (d3 + q(3,1)) * sin(q(2,1));
    J(3,3) = cos(q(2,1));



    xi = mm3aFk(q);       // obtaining intial position for configuration q
    zeroX = xi;                   // setting zero position as xi

    xiG = xi;                       // xi global for a set of input commands from the user
    xdG = zero3x1;           // global desired position that the user wants to reach
    xdGinPix = zero3x1;

    xdGinPix(1,1) = 50.0;
    xdGinPix(2,1) = 50.0;
    xdGinPix(3,1) = 50.0;

    proxy = xi;                    // setting proxy point to initial position
    
    // setting xdG. Replace by input from the user
    xdG(1,1)=43;
    xdG(2,1)=1.0;
    xdG(3,1)=0.0;
    

    tVect = xdG - xiG;         // calculating intial target vector from xdG and xiG
    


    // loading homography file
    CvMat *Htemp;
    // using temp matrix to load homography matrix
    Htemp = (CvMat*)cvLoad("../nanocalib/calibFiles/homography2.xml" );
    float *data;
    data = Htemp->data.fl;

    H = zero3x3;

    // transferring homography elements from temp matrix to H
    H(1,1) = CV_MAT_ELEM(*Htemp,float, 0,0 );
    H(1,2) = CV_MAT_ELEM(*Htemp,float, 0,1 );
    H(1,3) = CV_MAT_ELEM(*Htemp,float, 0,2 );
    H(2,1) = CV_MAT_ELEM(*Htemp,float, 1,0 );
    H(2,2) = CV_MAT_ELEM(*Htemp,float, 1,1 );
    H(2,3) = CV_MAT_ELEM(*Htemp,float, 1,2 );
    H(3,1) = CV_MAT_ELEM(*Htemp,float, 2,0 );
    H(3,2) = CV_MAT_ELEM(*Htemp,float, 2,1 );
    H(3,3) = CV_MAT_ELEM(*Htemp,float, 2,2 );

    // qDebug()<<H(1,1);

    // storing xdG and xiG in position_points array to plot initial and target position in display
    Matrix temp =  convertPixelstoMM(xiG,-1);
    qDebug() << __LINE__ << Q_FUNC_INFO << temp(1,1) << temp(2,1) << temp(3,1);
    qDebug() << __LINE__ << Q_FUNC_INFO << "Initial conditions:";
    qDebug() << __LINE__ << Q_FUNC_INFO << "q" << q(1,1) << q(2,1) << q(3,1);
    qDebug() << __LINE__ << Q_FUNC_INFO << "XiG "<<temp(1,1) << temp(2,1) << temp(3,1);

    CV_MAT_ELEM(*position_points, float, 0, 0 ) = temp(1,1);
    CV_MAT_ELEM(*position_points, float, 0, 1 ) = temp(2,1);

    temp = convertPixelstoMM(xdG,-1);
    qDebug()<<"XdG "<<temp(1,1) << temp(2,1);
    CV_MAT_ELEM( *position_points, float, 1, 0 ) = temp(1,1);
    CV_MAT_ELEM( *position_points, float, 1, 1 ) = temp(2,1);

    
    // printing out xiG and target vector to verify values
    qDebug()<<"XiG "<<xiG(1,1) << xiG(2,1) << endl << "tVect "<<tVect(1,1)<< tVect(2,1);
    
    port = new SerialWidget();              // initializing port for serial communication

    qDebug()<<"MM3AWidget:MM3AWidget(): Creating Object... Done";
}

/*
*   Summary:   MM3AWidget destructor
*
*   Parameters:     None
*   Returns:    None
*/
MM3AWidget::~MM3AWidget()
{
    //close();
}

/**  \brief   updates mm3a for given input q along joint

 @param q vector of joint inputs
 @param delta number of steps to be commanded
 @param updateMode selects if joint estimates are to be update or not
 @return updated position of mm3a
*/
Matrix MM3AWidget::updateMM3A(Matrix q, Matrix delta, int updateMode)
{   
    Matrix x = Matrix(3,1);
    
    // recalculating Jacobian
    J(1,1) = - (a1 - (d3+q(3,1))*sin(q(2,1)))*(-sin(theta1)); // + toolOffset*cos(theta1);
    J(1,2) = - (d3 + q(3,1)) * cos(q(1,1)) * cos(q(2,1));
    J(1,3) = - cos(q(1,1)) * sin(q(2,1));
    J(2,1) = (a1 - (d3+q(3,1))*sin(q(2,1))) * cos(q(1,1)) ; //+ toolOffset*sin(theta1);
    J(2,2) = - (d3 + q(3,1)) * sin(q(1,1)) * cos(q(2,1));
    J(2,3) = - sin(q(1,1)) * sin(q(2,1));
    J(3,1) = 0.0;
    J(3,2) = - (d3 + q(3,1)) * sin(q(2,1));
    J(3,3) = cos(q(2,1));

    // calculating change in joint configuration for input
    if(delta(1,1)>0.0) {
        dq(1,1)= (delta(1,1)- c-L2*q(2,1) - mA*mass)/mAaclk; // - 0.615;
        // qDebug()<<"Here1"<<delta(1,1);
    } else {
        // switching if displacement req is greater than for step 4
        dq(1,1) = (double)delta(1,1)/mAclk ; // - 0.565);
        // qDebug()<<"Here2"<<delta(1,1);
    }

    // find number of steps along link 3 using calibration model
    if(delta(2,1)>0) {
        dq(2,1) = (delta(2,1) - cCout - mC*mass)/mCout;// - 0.580;
        //        qDebug()<<"here";
    } else {
        dq(2,1) = delta(2,1)/mCin; //- 0.483);
    }

    dq = P * delta;

    switch(jointUpdateMode) {
    case 1:
        // for not updating jacobian part
        qf = q;

    case 0:
        // for normal operation
        qf = q + dq;
        //qDebug()<<"MM3AWidget::updateMM3A:qf"<<qf(1,1)<<qf(2,1)<<qf(3,1);
        break;
    }
    // updating using mm3aFK
    x = mm3aFk(qf);
    return x;
}

/** \brief   solves forward kinematics for the mm3a

    @param q joint vector

    @return :    updated position of mm3a
*/
Matrix MM3AWidget::mm3aFk(Matrix q)
{
    Matrix x(3,1);
    // calculating forward kinematics for mm3a.
    x(1,1) = (a1 - (d3+q(3,1)) * sin(q(2,1))) * cos(q(1,1)); // +toolOffset*sin(q(1,1));
    x(2,1) = (a1 - (d3+q(3,1)) * sin(q(2,1))) * sin(q(1,1)); //+toolOffset*cos(q(1,1));
    x(3,1) = d3 * cos(q(2,1));
    
    return x;
}

/** \brief   solves inverse kinematics for the mm3a using Jacobian
    @param xi current position
    @param q current joint vector
    @param proxy proxy point to be reached

    @return joint vector required to reach desired position
*/
Matrix MM3AWidget::mm3aIk(Matrix proxy, Matrix xi, Matrix q )
{
    // recalculating Jacobian
    J(1,1) = - (a1 - (d3+q(3,1))*sin(q(2,1)))*(-sin(theta1)); // + toolOffset*cos(theta1);
    J(1,2) = - (d3 + q(3,1)) * cos(q(1,1)) * cos(q(2,1));
    J(1,3) = - cos(q(1,1)) * sin(q(2,1));
    J(2,1) = (a1 - (d3+q(3,1))*sin(q(2,1))) * cos(q(1,1)) ; //+ toolOffset*sin(theta1);
    J(2,2) = - (d3 + q(3,1)) * sin(q(1,1)) * cos(q(2,1));
    J(2,3) = - sin(q(1,1)) * sin(q(2,1));
    J(3,1) = 0.0;
    J(3,2) = - (d3 + q(3,1)) * sin(q(2,1));
    J(3,3) = cos(q(2,1));

    // impulse jacobian matrix
    //qDebug()<<J(1,1)<<J(2,1)<<J(3,3)<<a1;
    //    P(1,1) = 4.18879/(58550+754*q(2,1)); //754
    //    P(2,2) = -4.18879/(58550);
    //    P(3,3) = 12.0/10550;
    Matrix Ji = J*P;

    // calculating error
    Matrix error = proxy-xi;

    Matrix Delta(3,1);
    //Matrix dQ(3,1);

    Delta = Ji.i()*error;

    dQ = J.i()*error;       // change in joint configuration required to achieve desired velocity
}

/** \brief calculates the number of steps required along each axis for desired end-effector velocity

  @param proxy proxy point
  @param xi current position
  @param current joint vector

  @return rounded value of number of steps along each joint
*/
Matrix MM3AWidget::findSteps(Matrix proxy, Matrix xi, Matrix q)
{
    Matrix Delta(3,1);
    Matrix rDelta(3,1);

    // recalculating Jacobian
    J(1,1) = - (a1 - (d3+q(3,1))*sin(q(2,1)))*(-sin(q(1,1))); // + toolOffset*cos(theta1);
    J(1,2) = - (d3 + q(3,1)) * cos(q(1,1)) * cos(q(2,1));
    J(1,3) = - cos(q(1,1)) * sin(q(2,1));
    J(2,1) = (a1 - (d3+q(3,1))*sin(q(2,1))) * cos(q(1,1)) ; //+ toolOffset*sin(theta1);
    J(2,2) = - (d3 + q(3,1)) * sin(q(1,1)) * cos(q(2,1));
    J(2,3) = - sin(q(1,1)) * sin(q(2,1));
    J(3,1) = 0.0;
    J(3,2) = - (d3 + q(3,1)) * sin(q(2,1));
    J(3,3) = cos(q(2,1));

    // step size models
    //    P(1,1) = 4.18879/(58550+754*q(2,1)); //754
    //    P(2,2) = -4.18879/(58550);
    //    P(3,3) = 12.0/10550;

    //    qDebug() << __LINE__ << P(1,1) << P(2,2) << P(3,3);

    Matrix Ji = J*P; // impulse Jacobian
    Matrix error = proxy-xi;     // calculating error


    Delta = Ji.i()*error;

    // find joint configuration changes
    dQ = J.i()*error;

    // rounded values for delta
    rDelta(1,1) = round(Delta(1,1));
    rDelta(2,1) = round(Delta(2,1));
    rDelta(3,1) = round(Delta(3,1));

    //    qDebug()<<"MM3AWidget()::mm3aIk:rDelta "<<rDelta(1,1)<<rDelta(2,1)<<rDelta(3,1) << "proxy "<<proxy(1,1);
    return rDelta;
}

/** \brief   Algorithm to calculate number of steps required along each joint to achieve desired end-effector velocity.

    This function also sends the step commands to the manipulator via a serial port. It converst the position in mm to pixels using the homography and store it in position_points. Since all experiments are performed in a 2D plane either vertical or horizontal, the magnitude and direction of velocity can be used to get the target position at each instant.

    @param velMag magnitude of input velocity
    @param velDir direction of input velocity

    @return
*/
void MM3AWidget::variableStepMag(double velMag, double velDir)
{
    //     qDebug()<<"variableStepMag"<<xdG(1,1)<< velMag << velDir;

    //    qDebug()<<"variableStepMag"<<xdG(2,1);

    
    // converting limiting velMag to 0.02
    if(velMag>0.018)
        velMag = 0.018;
    
    xdGinPix = convertPixelstoMM(xdG, -1);


    // defining target velocity
    Matrix velTarget(3,1);
    if(expPlane==0) {
        velTarget(1,1) = velMag * cos(velDir);
        velTarget(2,1) = velMag * sin(velDir);
        velTarget(3,1) = 0.0;
    } else {
        velTarget(1,1) = velMag * cos(velDir);
        velTarget(2,1) = 0.0;
        velTarget(3,1) =  -velMag * sin(velDir);
    }
    
    // update step size model
    updateStepSize(velTarget);



    // updating proxy point alg 1
    if(operatorMode == 0)
        proxy = xi + velTarget;
    
    // updating proxy point alg 2
    if(operatorMode == 1)
        proxy = proxy + velTarget;

    // finding number of steps required along each dof to reach the proxy point
    Matrix delta = findSteps(proxy, xi, q);


    // limiting n to maximum step size. should not be used as magnitude of velocity has alredy
    // been limited to satisfy this constraint
    int maxSteps = 100;
    if(delta(1,1)>maxSteps) {
        delta(1,1)=maxSteps;
        //qDebug()<<"MM3AWidget()::variableStepMag:delta 1 + limit";
    }
    
    if(delta(2,1)<-maxSteps) {
        delta(2,1)=-maxSteps;
        //qDebug()<<"MM3AWidget()::variableStepMag:delta 2 - limit";
    }

    if(delta(3,1)>maxSteps) {
        delta(3,1)=maxSteps;
        // qDebug()<<"MM3AWidget()::variableStepMag:delta 3 + limit";
    }

    if(delta(1,1)<-maxSteps) {
        delta(1,1)=-maxSteps;
        // qDebug()<<"MM3AWidget()::variableStepMag:delta 1 - limit";
    }

    if(delta(2,1)<-maxSteps) {
        delta(2,1)=-maxSteps;
        //qDebug()<<"MM3AWidget()::variableStepMag:delta 2 - limit";
    }
    
    if(delta(3,1)<-maxSteps) {
        delta(3,1)=-maxSteps;
        //qDebug()<<"MM3AWidget()::variableStepMag:delta 3 - limit";
    }
    
    // updating total step count
    stepCountA = stepCountA  + delta(1,1);
    stepCountB = stepCountB + delta(2,1);
    stepCountC = stepCountC + delta(3,1);


    
    // updating estimated position with selected input
    xf = updateMM3A(q, delta, 1);
    xPix = convertPixelstoMM(xf,-1);

    //    if(xPix(1,1) < xlimit[0] || xPix(1,1) > xlimit[1] || xPix(2,1) < ylimit[0] || xPix(2,1) > ylimit[1]) {
    //        proxy = xi;
    //    } else { }
    xi = xf;
    q = qf;

//        qDebug() << __LINE__ << Q_FUNC_INFO << "Estimated q: " << q(1,1) << q(2,1) << q(3,1);
//        qDebug() << __LINE__ << Q_FUNC_INFO << "Updated step sizes: " << P(1,1) << P(2,2) << P(3,3);

    XinPix = xPix;
    
    std::ostringstream vM;
    std::ostringstream vD;
    vM<< velMag;
    vD<< velDir;

    // save data to a file. Data save order: Calculated position     proxy      actual position
    fp_data = fopen("../nanocalib/saved/data.txt", "a");
    fprintf(fp_data, "%f %f %f %f %d %d \n", xf(1,1),xf(2,1), proxy(1,1), proxy(2,1), probeTip.x, probeTip.y);
    fclose(fp_data);


    // storing all the points in a matrix

    if(expPlane == 0) {
        CV_MAT_ELEM( *position_points, float, Scount, 0 ) = float(xPix(1,1));
        CV_MAT_ELEM( *position_points, float, Scount, 1 ) = float(xPix(2,1));
    } else {
        CV_MAT_ELEM( *position_points, float, Scount, 0 ) = float(xPix(3,1));
        CV_MAT_ELEM( *position_points, float, Scount, 1 ) = float(xPix(1,1));
    }

    Scount = Scount + 1;


    // sendcommand to nanoControl
    //    qDebug()<< __LINE__ << Q_FUNC_INFO << "delta"<<delta(1,1)<<delta(2,1)<<delta(3,1);
    sendCommand(delta);


    //velTarget.ReleaseAndDelete();
    //delta.ReleaseAndDelete();
}

/** \brief calculates angle error between two given vectors. Change function to take input as two vectors

    @param target vector

    @return error between two vectors in rad
*/
double MM3AWidget::calcAngleError(Matrix y)
{
    //Matrix B(3,1);
    Matrix xfvect(3,1);
    // calculating last movement vectors.
    xfvect = y - xiG;
    
    // calculating angle error between target vector and last movement vector for the mm3a
    double error = atan2(tVect(2,1),tVect(1,1)) - atan2(xfvect(2,1),xfvect(1,1));
    
    return error;
    
}

/** \brief   converts pixels to mm and mm to pixels using the scalingFactor

  @param x position vector in mm or pixels
  @param convMode flag for selecting conversion mode. -1 = mm to pixels, 1 = pixels to mm

*/
Matrix MM3AWidget::convertPixelstoMM(Matrix x, int convMode)
{
    Matrix converted(3,1);
    switch(convMode) {
    // mm to pixels
    case -1:
    {
        //x(1,1) = (x(1,1) - zeroX(1,1))*scalingFactor + center(1,1);
        //x(2,1) = (x(2,1) - zeroX(2,1))*scalingFactor + center(2,1);

        if(expPlane == 0) {
            converted(1,1) = x(1,1)-zeroX(1,1);
            converted(2,1) = x(2,1)-zeroX(2,1);
            converted(3,1) = 1.0;
        } else {
            converted(1,1) = x(3,1)-zeroX(1,1);
            converted(2,1) = x(2,1)-zeroX(2,1);
            converted(3,1) = 1.0;
        }

        converted = H * (converted);
    }
        break;
        
        // pixels to mm. Not used
    case 1:
        //x(1,1) = (x(1,1) - center(1,1))/scalingFactor + zeroX(1,1) ;
        //x(2,1) = (x(2,1) - center(2,1))/scalingFactor + zeroX(2,1);
        converted(1,1) = x(1,1) + center(1,1);
        converted(2,1) = x(2,1) + center(1,1);
        converted(3,1) = 1.0;
        Matrix HInv;
        HInv = H.i();
        converted = HInv.t() * (converted);
        break;
    }

    x(1,1) = converted(1,1);
    x(2,1) = converted(2,1);
    return x;
}

/** \brief   sends command to the nanocontrol unit

    @param delta rounded step commands to be sent to the manipulator
*/
void MM3AWidget::sendCommand(Matrix delta)
{
    // variables for serial communication
    QString coarse = "coarse";
    QString dir1;
    QString dir2;
    QString dir3;
    QString command1;
    QString command2;
    QString command3;
    QString jointA = " A";
    QString jointB = " B";
    QString jointC = " C";
    
    if(delta(1,1)>0)
        dir1 = "-";
    else
        dir1 = "+";

    if(delta(2,1)>0)
        dir2 = "-";
    else
        dir2 = "+";

    if(delta(3,1)>0)
        dir3 = "-";
    else
        dir3 = "+";


    //    qDebug() << __LINE__ << delta(1,1) << delta(2,1) << delta(3,1);
    if (!simulate_status) {
        // send command for link1
        if(delta(1,1)!=0) {
            command1 = coarse + jointA + dir1 + QString::number(abs(delta(1,1)));
            //            qDebug() << __LINE__ << command1;
            // send command to nanocontrol
            port->sendCommand(command1);
            // wait for feedback from nanocontrol. This tells if the command has be executed or not
            port->receiveStatus();
        }

        //        qDebug() << __LINE__ << command2;
        if(delta(2,1)!=0) {

            // send command for link2
            command2 =  coarse + jointB + dir2 + QString::number(abs(delta(2,1)));

            port->sendCommand(command2);
            // wait for feedback from nanocontrol. This tells if the command has be executed or not
            port->receiveStatus();
        }

        //        qDebug() << __LINE__ << command3;
        if(delta(3,1)!=0) {
            // send command for link3
            command3 =  coarse + jointC + dir3 + QString::number(abs(delta(3,1)));

            port->sendCommand(command3);

            // wait for feedback from nanocontrol. This tells if the command has be executed or not
            port->receiveStatus();
        }
        // qDebug()<<"MM3AWidget()::sendCommand()<<command 1 command 2"<<command1<<command2<< command3<<"\n";
    }
}

/** Summary:   resets experiment // Not used
*/
void MM3AWidget::reset()
{
    
    simulate_status = 0;
    
    Matrix A(3,3);
    Matrix B(3,1);
    B << 0 << 0 << 0;
    
    // initializing q
    q(1,1) = 0.0;
    q(2,1) = 1.57;
    q(3,1) = 0.0;
    
    // initializing Jacobian values
    J(1, 1) = - (a1 - (d3+q(3,1))*sin(q(2,1)))*(-sin(theta1)); // + toolOffset*cos(theta1);
    J(1, 2) = - (d3 + q(3,1)) * cos(q(1,1)) * cos(q(2,1));
    J(1, 3) = - cos(q(1,1)) * sin(q(2,1));
    J(2, 1) = (a1 - (d3+q(3,1))*sin(q(2,1))) * cos(q(1,1)) ; //+ toolOffset*sin(theta1);
    J(2, 2) = - (d3 + q(3,1)) * sin(q(1,1)) * cos(q(2,1));
    J(2, 3) = - sin(q(1,1)) * sin(q(2,1));
    J(3, 1) = 0.0;
    J(3, 2) = - (d3 + q(3,1)) * sin(q(2,1));
    J(3, 3) = cos(q(2,1));
    

    // resetting step size matrix
    P = zero3x3;
    updateStepSize(zero3x1);
    
    
    // obtaining intial position for configuration q
    xi = mm3aFk(q);
    
    // setting zero position as xi
    zeroX = xi;
    
    // xi global for a set of input commands from the user
    xiG = xi;
    // global desired position that the user wants to reach
    xdG = B;
    
    // setting proxy point to initial position
    proxy = xi;
    
    // setting xdG. Replace by input from the user
    xdG << 43<<1.0<<0.0;
    
    cvReleaseMat(&position_points);
    
    position_points = cvCreateMat(100000, 2, CV_32FC1);
    
    // storing xdG and xiG in position_points array to plot initial and target position in display
    Matrix temp =  convertPixelstoMM(xiG,-1);
    qDebug()<<"XiG "<<temp(1,1) << temp(2,1);
    CV_MAT_ELEM( *position_points, float, 0, 0 ) = temp(1,1);
    CV_MAT_ELEM( *position_points, float, 0, 1 ) = temp(2,1);
    
    temp = convertPixelstoMM(xdG,-1);
    qDebug()<<"XdG "<<temp(1,1) << temp(2,1);
    CV_MAT_ELEM( *position_points, float, 1, 0 ) = temp(1,1);
    CV_MAT_ELEM( *position_points, float, 1, 1 ) = temp(2,1);
    
    // counting number of points
    Scount = 2;
}

void MM3AWidget::recordPoints()
{
    if(rCount<=4) {
        qDebug()<<"rCount"<<rCount;
        recPoints(1,rCount) = xi(1,1);
        recPoints(2,rCount) = xi(2,1);
        rCount++;
    } else {
        qDebug()<<"Max number of points recorded";
        qDebug()<< "Point 1 = "<<recPoints(1,1) << recPoints(2,1);
        qDebug()<< "Point 2 = "<<recPoints(1,2) << recPoints(2,2);
        qDebug()<< "Point 3 = "<<recPoints(1,3) << recPoints(2,3);
    }
}

/** \brief Updates the step size for the joints of the MM3A based on velocity input

  Step size matrix P is updated intrinsically

  @param velTarget Cartensian input velocity

*/
void MM3AWidget::updateStepSize(Matrix velTarget)
{
    // manipulator configuration angles. Refer Aayush thesis pg. 19
    double psi = 0 ;
    double eta = psi - q(2,1);
    double thetaManip = 0;


    // x-axis -> joint 3. y-axis -> joint 1 or 2 depending on experiment plane.
    if(modelMode == 1) {


        if(velTarget(1,1)>=0) {
            P(3,3) = ((1 - bPlus * abs(cos(q(2,1)))) * (aPlus - cPlus * cos(eta) * cos(thetaManip))) * 0.000001;
        } else {
            P(3,3) = ((1 - bMinus * abs(cos(q(2,1)))) * (aMinus - cMinus * cos(eta) * cos(thetaManip))) * 0.000001;

        }

        //        qDebug() << __LINE__ << Q_FUNC_INFO << P(3,3);

        if(expPlane==0) {
            if(velTarget(2,1) >= 0)
                P(1,1) = stepSizeA;
            else
                P(1,1) = stepSizeA;
        }

        if(expPlane == 1) {

            if(velTarget(3,1)>=0) {
                d2Plus = d2q30Plus + q(3,1) * ((d2q312Plus - d2q30Plus)/q3Range);
                P(2,2) = ((g2q30t_90p0Plus + d2Plus * sin(-q(2,1)))) * 0.000001 ;
            } else {
                d2Minus = d2q30Minus + q(3,1) * ((d2q312Minus - d2q30Minus)/q3Range);
                P(2,2) = ((g2q30t_90p0Minus+ d2Minus * sin(-q(2,1))))  * 0.000001;

            }

        }



        //        qDebug() << __LINE__ << Q_FUNC_INFO <<  g2q30t_90p0Plus << g2q30t_90p0Minus;
      //         qDebug() << __LINE__ << P(1,1) << P(2,2) << P(3,3);

    }

    if(modelMode == 0) {
        P(1,1) = stepSizeA;
        P(2,2) = stepSizeB;
        P(3,3) = stepSizeC;

    }
}

void MM3AWidget::setModel(int index)
{
    modelMode = index;
    qDebug()<<"MM3AWidget::setModel(): "<<modelMode;
}

void MM3AWidget::setOperatorMode(int index)
{
    operatorMode = index;
    qDebug()<<"MM3AWidget::setOperatorMode(): "<<operatorMode;
}



void MM3AWidget::setJointUpdateMode(int index)
{
    jointUpdateMode = index;
    qDebug() << __LINE__ << Q_FUNC_INFO << jointUpdateMode;
}



int MM3AWidget::getOperatorMode()
{
    return operatorMode;
}

int MM3AWidget::getModelMode()
{
    return modelMode;
}

void MM3AWidget::setEstCorner(int cornerCount)
{
    estCorner(cornerCount, 1) = xPix(1,1);
    estCorner(cornerCount , 2) = xPix(2,1);

    // qDebug()<< "Corner set " << estCorner(cornerCount, 1)<<estCorner(cornerCount, 2) << cornerCount;
}
